#include "motor.h"
#include "arduino.h"
#include "HX711.h"
#include "DengFOC.h"
#include <SimpleFOC.h>     //仅用了simplefoc的传感器部分

int Sensor_DIR=1;     //传感器方向
int Motor_PP=7;       //电机极对数


int32_t motor_gui_position = 0;
extern MagneticSensorSPI sensor;
extern float motor_target;

                                                                                                                                                                                                                                                                  
void motor_task(void *pvParameters)
{

  DFOC_Vbus(4.2);   //设定驱动器供电电压
  DFOC_alignSensor(Motor_PP,Sensor_DIR);

  while (1)
  {
    
    motor_gui_position = sensor.getAngle();  //带圈数的角度 

    Serial.print("sensor angle:  ");    
    Serial.print(sensor.getAngle());

    Serial.print("   velocity:  ");    
    Serial.println(sensor.getVelocity());
   
   // float Kp=2.68;
  //  setTorque(_constrain(Kp*(motor_target-Sensor_DIR*sensor.getAngle()),-2.1,2.1),_electricalAngle());
  //  serialReceiveUserCommand(); 


  //设置速度环PID
   DFOC_M0_SET_VEL_PID(0.0008, 0.000001, 0.0000015, 0);
  //设置速度
   DFOC_M0_setVelocity(serial_motor_target());
  //接收串口
  serialReceiveUserCommand();

  }
   vTaskDelay(10);
}

void motor_init()
{
  xTaskCreatePinnedToCore(motor_task, "motor_task", 8 * 1024, NULL, 1, NULL, 0);
}

